package kit.edu.lego.gruppe3.levels.rollbridge;

import kit.edu.lego.gruppe3.ComponentsRepository;
import kit.edu.lego.gruppe3.LegoMain;
import kit.edu.lego.gruppe3.actions.Orientation;
import kit.edu.lego.gruppe3.levels.LevelSelector;
import kit.edu.lego.gruppe3.sensors.GroundType;
import kit.edu.lego.gruppe3.sensors.Scanner;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Behavior;

public class RollBridgeEnd implements Behavior
{

	private DifferentialPilot robot = ComponentsRepository.getInstance()
			.getRobotPilot();

	@Override
	public void action()
	{
		robot.stop();
		robot.setTravelSpeed(5);
		robot.forward();
		while (robot.isMoving())
			if (Scanner.getGroundTypeAt(0,true)==GroundType.Line)
				robot.stop();
//		Orientation.orientAtType(GroundType.Line);
		Scanner.getGroundTypeAt(60);
//		robot.rotate(-10);
		robot.steer(20);
		while (robot.isMoving())
			if (Scanner.getGroundTypeAt(0,true)==GroundType.Line)
				robot.stop();
		RollBridgeSolver.setLevelCleared(true);
		LegoMain.startLine();
	}

	@Override
	public void suppress()
	{

	}

	@Override
	public boolean takeControl()
	{
		return LevelSelector.getInstance().isRollBridge()
				&& Scanner.getGroundTypeAt(0,true)==GroundType.Black;
	}

}
